Single Inverted Pendulum¶

First we need the model of the single inverted pendulum. Friedland works it mostly out for us in Chapter 2.

This looks reasonable for hanging down at least.

(The nonlinear state equations, which come from (2E.3) for $\ddot y$ and $\ddot \theta$ are figured out below. )

This is with the pendulum driven by a force. In problem 2.2 which you did in Homework 2, we have the model when driven by an electric motor, which is what we have with our hardware. Here is the solution from Joe Dinius.

Problem 2.1¶

The cart pendulum equations-of-motion are:

$$ \ddot y + \frac{mg}{M} \theta = \frac{f}{M} \\ \ddot \theta - \frac{M+m}{M \ell} g \theta = -\frac{f}{M \ell} $$

$\tau = rf$, $r$ is the torque-to-linear-force factor. For the electric motor:

$$ \tau = -\frac{k^2}{R} \omega + \frac{K}{R}e \\ = rf \\ \implies f = -\frac{k^2}{Rr} \omega + \frac{K}{Rr}e $$

The linear acceleration, $\ddot y$, is equal to $\ddot y = r \dot \omega \implies \dot y = r \omega$ by Newton's law. This implies: $$ f = -\frac{k^2}{Rr^2} \dot y + \frac{K}{Rr}e $$

Plug in $f$ to the first equation: $$ \ddot y + \frac{mg}{M} \theta = -\frac{k^2}{MRr^2} \dot y + \frac{K}{MRr}e \\ \ddot \theta - \frac{M+m}{M \ell} g \theta = \frac{k^2}{MRr^2 \ell} \dot y - \frac{K}{MRr \ell}e $$

Move state derivatives to the LHS: $$ \ddot y +\frac{k^2}{MRr^2} \dot y + \frac{mg}{M} \theta = \frac{K}{MRr}e \\ \ddot \theta - \frac{k^2}{MRr^2 \ell} \dot y - \frac{M+m}{M \ell} g \theta = - \frac{K}{MRr \ell}e $$

The next step is to take this linearized state model and put it into state form. I am going to put the variable $s$ into the equations where $s=1$ for the pendulum up, and $s= -1$ for the pendulum down.

$$\begin{bmatrix} \dot y \\ \dot \theta \\ \ddot {y} \\ \ddot {\theta} \end{bmatrix} = \begin{bmatrix} 0 && 0 && 1 && 0 \\ 0 && 0 && 0 && 1 \\ 0 && -mg/M && -k^2/(MRr^2)&& 0 \\ 0 && s(M+m)g/(Ml) && sk^2/(MRr^2l) && 0 \end{bmatrix} \begin{bmatrix} y \\ \theta \\ \dot{y} \\ \dot {\theta} \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ K/(MRr) \\ -sK/(MRrl) \end{bmatrix} e $$

so $$\mathbf A = \begin{bmatrix} 0 && 0 && 1 && 0 \\ 0 && 0 && 0 && 1 \\ 0 && -mg/M && -k^2/(MRr^2)&& 0 \\ 0 && s(M+m)g/(Ml) && sk^2/(MRr^2l) && 0 \end{bmatrix}$$ and $$\mathbf B = \begin{bmatrix} 0 \\ 0 \\ K/(MRr) \\ -sK/(MRrl) \end{bmatrix}$$

For an ideal motor, the torque constant is equal to the back electromotive force constant, so $k = K$. The applied voltage is $e$. The mass of the cart is $M$; the resistance of the motor is $R$; the radius of the wheels (or pulley in our case) is $r$; and the mass of the pendulum is $m$, with $g$ for gravity. For our pendulum, we might need to re-write these in terms of their moments of inertia sometime.

For now let's assume all the states are available, so $$ \mathbf C = \begin{bmatrix} 1 && 0 && 0 && 0 \\ 0 && 1 && 0 && 0 \\ 0 && 0 && 1 && 0 \\ 0 && 0 && 0 && 1 \end{bmatrix}$$

Let's see if our equations are good by simulating the system to see if it works like we expect.

Unstable Pendulum (Standing up)¶

Let's begin with an investigation when the pendulum is standing up, not hanging down. The model allows this with $s=1$.

Steve Brunton has some interesting things to say about how controllable a system is on his video here. I want to check the single pendulum to see how controllable it is. Steve describes how you can tell what directions are more controllable by doing the singular value decomposition of $\mathbf Q$ because Grammian $\mathbf W_t \approx \mathbf {Q Q}^T$.

The second eigenvector is the unstable one. Note its direction. Then look at the least controllable direction. Note its direction. We don't want them in the same direction. To find the angle between them, we find the dot or inner product between them. They both appear to be unit vectors the way python calculates them.

The inner product is small and the length of the vectors that were compared is unity, so that is good! It means they are practically orthogonal. Now let's check to see if we place the poles at the same pole locations we started with, the gain coefficients are all come out very close to zero. This is just a check of our the place algorithm.

Stable Pendulum Simulation (Hanging Down)¶

Remember this is the result of a simulation of a linear model. The question to ask is, "Is this accurate?" To check this out, remember the approximations we made were $cos \theta \approx 1$ and $sin\theta \approx \theta$. For the largest angles we see this works out to be:

This isn't the only source of error in our simulation. There are at least a couple others. One is due to not knowing the actual parameters very precisely. An approximate error analysis can be had by looking at the elements in the $A$ matrix to see how they vary as a function of each error. For example if a coefficient, $a_{ij}=f(m,M,r,R,k,K,l)$ then the error is approximately: $$da_{ij} = \frac {\partial a_{ij}} {\partial m} dm+ \frac {\partial a_{ij}} {\partial M} dM+\frac {\partial a_{ij}} {\partial r} dr+\frac {\partial a_{ij}} {\partial R} dR+\frac {\partial a_{ij}} {\partial k} dk+\frac {\partial a_{ij}} {\partial K} dK+\frac {\partial a_{ij}} {\partial l} dl$$

The only reason it damps out is the resistance of the motor windings. We modeled no friction. The underdamped responses (from complex poles) are expected. There could be a damping effect from friction as well. To tell if we should add that to the model, the thing to do is to actually hang the pendulum down, and see if the decay time of the oscillating wave is consistent with what the linear model predicts.

Not changing the pole locations results in a zero gain matrix as expected.

Let's see if we can change the behavior of the pendulum so it is like it was in honey, dieing out in 1, 2, 3, and 4 seconds. We will place the poles at -1, -2, -3 and -4. Then do the simulation.

Note how the cart moves to keep the natural swinging behavior from happening. The reason it looks a little oscillatory is because some eigenmodes damp out faster and some slower.

Note that the maximum angle is still $0.1^o$, so our previous check of the angles still applies.

It looks like we were successful placing the poles where we wanted them.

I would like to know how much difference friction is making for the pendulum. The simulation above may seem like a good place to start, but it assumes the input voltage is zero (shorted), but in that configuration, the resistance of the motor windings, $R$, causes damping. So it would be nice to have a model where the current through the motor is zero. This configuration would have no damping due to $i^2R$ losses in the motor.

Zero Current Input Model¶

In this case, the torque $\tau$ is zero, because there is no Lorentz force with zero current. This is as if the force was set to zero in the original pendulum model before incorporating the motor. In this case the state model is:

With the s variable included this becomes:

$$\begin{bmatrix} \dot y \\ \dot \theta \\ \ddot {y} \\ \ddot {\theta} \end{bmatrix} = \begin{bmatrix} 0 && 0 && 1 && 0 \\ 0 && 0 && 0 && 1 \\ 0 && -mg/M && 0 && 0 \\ 0 && s(M+m)g/(Ml) && 0 && 0 \end{bmatrix} \begin{bmatrix} y \\ \theta \\ \dot{y} \\ \dot {\theta} \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \end{bmatrix} u $$

This is like setting $k=K=0$ in the model. Let's see if that does not decay, as we think it should. It should also give us a better measurement of the viscous friction in the real system.

We should check the real pendulum and make sure the pendulum period is $T_p = (2\pi)/5.78144 = 1.0868$ seconds, assuming the damping is negligible. If not, we should see what happens to this frequency as we add damping, and adjust our parameters so the period is correct based on what we think may be amiss.

Model Verification and Adjustment¶

In this section we devise various ways to verify the model parameters and modify the model as seems best. To do this we use the pendulum hanging down using the models for that developed above to verify/adjust and adjust the parameters for $\theta$, and then we put the pendulum on it's side and make a model to verify/adjust the motor parameters.

To do this we need to understand how the WWU pendulum works and how to operate it. This the the WWU inverted pendulum.

The pendulum motors and shaft angle encoders are controlled by a dedicated digital controller using a Xylinx FPGA that communicates with the user over Ethernet. To control it you use Ralph's code from the ctrlbox.m file below. You have to setup your computer to communicate with the pendulum at address 169.254.0.100. Here are the settings for Ubuntu.

You can check it works as shown below (on linux). If you can ping the pendulum, you can communicate with it.

There are two physical pendula. They have slightly different characteristics, and so the constants are different for each.

The pendulum needs to know where down is, and where the center of the track is as well. To set those you hit the reset button on the controller. Sometimes you cannot ping the pendulum, and if you reset it, you can. Here is a photo showing which button is the reset button.

The ctrlbox.m file is shown below. It contains the octave/MATLAB routines to communicate with the pendulum. Ralph Stirling is the one who wrote them, and he is also the one who wrote the FPGA controller code to communicate with the Wiznet Ethernet device that the controller uses to send the data over Ethernet.

Now we test the pendulum hanging down to see how it matches our model.

Here is a routine to test the pendulum. This helps us understand how things work. You need to install the octave-sockets package if you don't have it already.

The results from this simulation done with the motor open circuited, so the $R$ has no effect on the slow down of the pendulum seem to indicate that the major contribution to the pendulum friction is not viscous friction. It seems to be a constant torque opposing motion, and it seems to drop off linearly with time instead of exponentially. This paper analyzes this kind of motion. Because it has the term $sgn(\dot \theta)$ in the torque because it changes sign when the angular velocity does, it would require changing models when $\dot \theta$ changes sign. I don't want to mess with that at the moment. I suspect the friction due to the cart moving on the rail is a bigger deal based on just feeling it with my fingers. On that one, I'm not quite sure how to measure it, and I think it will likely also be a force with a $sgn(\dot y)$, I'm going to leave that out of my model too. It may be possible to model those forces as proportional to $\dot \theta$ and $\dot y$, but they would give an exponential decay, and at least for the pendulum, it isn't exponential. However, I do note from the paper that the frequency of oscillation is not a function of the friction, and I also note that the simulation and the measurement are a bit different in the frequency of oscillation. I will take that into account by adjusting the length $l$ to the center of mass of the pendulum. It appears that we measure five periods in about $4.7$ seconds, so the angular frequency is $2 \pi 5/4.7= 6.68$ radians per second. Our simulation has nine periods in about $9.8$ seconds, so that gives an angular frequency of $2 \pi 9/9.8 = 5.7703$ which reminds me we can just get it from the eigenvalue, which was $5.78144$. The characteristic polynomial of the $A$ matrix without the motor connected is $(Mls^4+Mgs^2+gms^2)/(Ml)$. This makes it obvious there are two poles at zero, and the remainder of the poles satisfy $(Mls^2+Mg+gm)=0$ and so the poles are at $\pm j\sqrt{g(1+m/M)/l}$ and this agrees with what octave calculated above. I am not quite sure why, but it seems the of moment of inertia is not quite right, so I am going to reduce the length of the rod to get agreement. The factor for which the resonant frequency is wrong is $6.68/5.78 = \sqrt{l_{old}/l_{new}} = \sqrt{0.323/l_{new}}$ so $l_{new}$ is calculated below as 0.242. The analysis below verifies this change fixes things.

To start the pendulum, you need to let it hang down until it isn't moving. Then you push the calibrate button on the FPGA board to tell it that is hanging down. Then you set the initial condition and start your program. The pause statement in octave lets you start it when you want with a single keystroke.

Cart Falling Down¶

The force the motor develops is important. We need to verify it, especially as some of the code from previous years seems to indicate that we might not have a good handle on the scale constant for the motor drive.
If $J$ is the moment of inertia of the system:

There is a minus sign missing in the $B$ matrix above.

The moment of inertia is made up of two parts, one from the motor and one from the mass so $J = J_M + \tilde{m}r^2$, and $\tilde m = m + M$, the total mass of the cart and pendulum.

A good test would be to see what motor voltage would hold the cart and pendulum up statically. This is also useful, because we can do some dynamic tests around that operating point too if we like. In this case, $\dot \omega = 0$, $\omega = 0$, and the back emf is also zero. $ke = mgr$, and the voltage to hold the pendulum up should be $e = mgrR/(k) = (0.3163 + 0.0318)(9.81)(0.0254/2)/0.0351$.

If there are no mistakes here, my experiments seem to indicate that the motor constant is $0.0351/1.7$. (I need to send 1.7 times as much voltage as I calculate using the old motor constant.) If I adjust the motor constant because of this, it becomes $0.0315/7.5$. This is somewhat surprising and needs more verification somehow.

State Feedback Hanging Down¶

Let's try to make the real pendulum die down with eigenmodes, $e^{-t}, e^{-2t}, e^{-3t},$ and $e^{-4t}$. We have the state feedback for that computed above. We will recalculate the $G$ gain, because we changed the effective length of the pendulum and the motor constant, $k$.

Playing with this seems to indicate that it is very insensitive to position in real life. Note the places of zero velocity in the results. This is non-linear behavior. I believe it is due to friction.

Friction Musings¶

I wonder if a good way to account for the friction is to make a model for travelling in each direction, and have a constant friction force in each direction that just changes sign with each direction change. We could design the control for each direction, and then just change the value when the sign of the movement changes. A thing that worries me is that the Q point is exactly where we would change the model. I suppose we could control at that point as if there was no friction at all, but actually the static friction is probably even greater than the friction for motion, and when it is static, I cannot use the direction of motion to determine its direction. I do know the force of the motor in the static case, so it might be possible to model the friction using Coulomb's friction relation, $F = \mu N$. The cart friction seems greater than the pendulum friction, so that is my principle concern, but the two seems very similar in the way they work, so solving one will probably solve the other. If we add this frictional force for the cart, it will be:

$$ f_{F} = \begin{cases} \mu_d(M+m) & \dot y > 0 \\ - \mu_s(m+M) sgn(f_M) & \dot y = 0 \\ -\mu_d(M+m) & \dot y < 0 \end{cases} $$

where $f_F$ is the force of friction, $f_M$ is the force of the motor, $M$ is the mass of the cart, $m$ is the mass of the pendulum, $\mu_s$ is the static friction, and $\mu_d$ is the dynamic friction, $\dot y$ is the velocity of the cart and $f_M$ is the force of the motor. The appropriate equations we need to add the force of friction into are: $$ $$ \ddot y + \frac{mg}{M} \theta = \frac{f}{M} \ \ddot \theta - \frac{M+m}{M \ell} g \theta = -\frac{f}{M \ell} $$ $$ The force $f$ now (including friction) needs to be the force due to the motor plus the frictional force. $$ f \rightarrow f_M + f_F$$ $\tau = rf$, $r$ is the torque-to-linear-force factor, the radius of the pulley. For the electric motor: $$ $$ \tau_M = -\frac{k^2}{R} \omega + \frac{K}{R}e \ = rf_M \ \implies f_M = -\frac{k^2}{Rr} \omega + \frac{K}{Rr}e $$ $$ The linear acceleration, $\ddot y$, is equal to $\ddot y = r \dot \omega \implies \dot y = r \omega$ by Newton's law. This implies: $$ $$ f_M = -\frac{k^2}{Rr^2} \dot y + \frac{K}{Rr}e $$ $$

Plug in $f$ to the first equation: $$ $$ \ddot y + \frac{mg}{M} \theta = -\frac{k^2}{MRr^2} \dot y + \frac{K}{MRr}e + \frac{f_F}{M}\ \ddot \theta - \frac{M+m}{M \ell} g \theta = \frac{k^2}{MRr^2 \ell} \dot y - \frac{K}{MRr \ell}e -\frac{f_F}{M\ell} $$ $$ Move state derivatives to the LHS: $$ $$ \ddot y +\frac{k^2}{MRr^2} \dot y + \frac{mg}{M} \theta = \frac{K}{MRr}e + \frac{f_F}{M}\ \ddot \theta - \frac{k^2}{MRr^2 \ell} \dot y - \frac{M+m}{M \ell} g \theta = - \frac{K}{MRr \ell}e -\frac{f_F}{M\ell} $$ $$ Now we need to go about measuring the friction coefficients, $\mu_d$, and $\mu_s$. Then we need to code the frictional force into a piecewise model. To measure $\mu_s$ we could just start the cart an a stationary position, and increase the motor force until it barely moves. To measure $\mu_d$ we will tilt the pendulum up until the cart slides down the incline, and measure the position as a function of time. This should give us a pretty good idea of the dynamic frictional force.

The plan for measuring the Coulomb friction coeficients, $\mu_s$ and $\mu_d$ is to set the cart on an angle and measure $y$. The static friction can be determined from the maximum angle before which the cart moves and the dynamic friction from the acceleration when the cart does move. See the diagram below.

We will gradually raise the pendulum up until the cart rolls. We will record the angle that it rolls. The script below will record $y$ and $\dot y$.

The height to get it going is 15 cm. The horizontal dimension of the carriage is 34 cm, so $sin(\theta) = 15/34$. You need to make this test with no motor connected so it doesn't slow the cart down. The angle is calculated below. I thinkI should hold the carriage at a higher angle to measure $\mu_d$. The friction is a function of the position of the carriage, $y$. Doing this will help overcome that problem.

Experiments show that the cart will not move when the rail is horizontal until the pwm number to the cart is between 3000 and 3500. This indicates the static friction motor voltage is about:

This piecewise linear approach seems to have been used by people all the way back to Kalman. The theory is beyond the scope of this class, but trying it out isn't.

Let's try to simulate it using octave's ode45 function. First we need a force function.

Now let's make a right hand side. Then let's simulate it.

This looks like what we observe. I need to compare the frequency of oscillations the experimental results. Note the flat places in the cart velocity, $\dot y$.

Full Order Observer (Hanging Down)¶

The full order observer is a system that monitors the actual system, taking as inputs the input of the actual system and the output of the system, and it's state, $\hat x$ is made to simulate the system so that the error, $e = (x-\hat x)$ tends to zero as with the eigenvalues we wish it to have. That means that $\hat e=\tilde A e$. Here is a quick derivation of how it works. The whole purpose of it is that sometimes we don't have access to all the states because they are not measured, or they are measured only with a lot of noise, and we want a better thing. We then use the estimates $\hat x$ instead of $x$ for the control.

To build a full order observer, we begin by deciding where we want the poles of the observer to be placed, and then use the place routine in octave to find the $K$ gain matrix that will do that. Then you just build the simulator system into the control program.

For my test, I want to compare the simulated and measured states. The pendulum actually measures the angle, $\theta$ and the position $y$. Note the unfortunate circumstance that the canonical variables $y$ and $e$ now have two interpretations each. I'm sorry about that. I'm trying to use the variables most often used in the textbook and literature. First we need to check the observability matrix and see if our pendulum system is observable.

So it is observable. Now let's find the gain matrix, $K$. Note this is like placing the poles for the controller if we use the transpose, because the eigenvalues are not changed by taking the transpose of the matrix, and $(KC)^T = C^T K^T$. We want the equation to match the one we used to place the poles of the controller and it does by doing the transpose, so we just place the poles of the transpose.

Let's investigate how this should work in theory with a simulation.

Note this simulation is not feeding the simulated values back using state feedback yet. We want to see how the observer converges before we take that step, so the simulation above is appropriate.

This did not include friction. It would be interesting to include friction in the simulation of the observer to see what happens to the observer outputs. Based on the real world results below, it would not work very well. This means we want to think about how we might include friction in the observer itself.

Now let's modify the control code so we can try what we did above on the real system (without friction). To do this, to start with, I am going to use a simple Euler approximation for $\dot {\hat x}(kT) \approx [\hat x((k+1)T)-\hat x(kT)]/T $. With this, $$ \dot {\hat x} = \hat {A} \hat x + \hat B u + KC x $$ becomes
$$ \hat {x}((k+1)T) \equiv \hat x (k+1) \approx (I+ \hat {A} T)\hat {x}(k) + B u(k) + KCx(k)$$

Adding Friction to the State Feedback in the Real Pendulum Hardware¶

The $A$ matrix is not changed by the friction. The $B$ matrix is not able to simulate the constant frictional forces that depend only on the signs of $\dot y$ and $f_M$. This means the way to compensate for friction is to add in an equal and opposite force using the motor.

Recall $\tau = rf_M$, $r$ is the torque-to-linear-force factor (radius of the pulley). For the electric motor: $$\tau = k i$$ $$ i = \frac{e-v}{R}$$ $$ v = k\omega$$ so putting that all together $$ \tau = \frac{-k^2\omega}{R} + \frac{ke}{R}$$ or $$ f_M = \frac{\tau}{ r} = \frac{-k^2\omega}{rR} + \frac{ke}{Rr}$$ We want to add a "constant" anti-friction force to the motor force by changing $e$, or equivalently add an anti-friction torque, $\tau$. The value of the force is $$f_{AF} = -f_F = \frac{ke_{AF}}{Rr}$$ so the extra vlotage we need to add is $$e_{AF} = \frac{-rRf_F}{k}$$

State Feedback With the Unstable Pendulum¶

Let's now go back to the unstable pendulum and try to place the poles where we want them. We will need to use a numerical approximation for the derivative to get the state variables for $\dot y$ and $\dot \theta$ for the actual pendulum, but first, let's simulate state feedback.

It might be good to use the LQR regulator to choose the poles to start with. A good article on that is this one from the University of Texas at Arlington. It explains how to pick the $Q$ and $R$ matrices. The $Q$ matrix tells how much energy you want the states to have (how quickly you want them to approach zero) and the $R$ matrix tells you how much energy you want to expend to get them there. The relative sizes of each of these elements is the key.

Homework 12 Help¶

We will do this problem for the WWU pendulum. First we need to see if the system is observable with just one state, and if so, what state will work. For $\theta$ as the measured variable, we have $C=\begin {bmatrix} 0 && 1 && 0&&0 \end {bmatrix}$. We need to compute the rank of the observability matrix, $R$, with this $C$.

That is not observable. Looks like we can't measure $y$. If we measure $y$ instead, is it observable?

That is observable! So we don't have to swap the order of the state variables! Nice!

Let's find where the poles need to be placed for the observer by finding $L$.

We need to find $\bar {\bar G}$ and $H$, but those are just "plug and chug." Rumor is they turn out to be zero.

Inverted Pendulum¶

Now that we have somewhat of a handle on the friction of the cart, let's try balancing the pendulum.